/**
 * File: robot_sensor_node.cpp
 * Brief: this node is work for receive sensor data from robot, and 
 *        transmit it to the corresponding topic.
 * Created by: zhangping
 * Created at: 2017.07.6
 * Modified by:
 * Modified at:
 **/

#include <stdio.h>
#include "ros/ros.h"
#include "protocol.h"
#include "communication.h"
#include "sensor_msgs/LaserScan.h"
#include "robot_msgs/RobotPosition.h"
#include "robot_msgs/RobotGeomagnetic.h"


/**
 * do work.
 */
void do_pos(ros::Publisher &pub, U8 x, U8 y, U8 z){
	
	robot_msgs::RobotPosition pos_msg;
	pos_msg.sensor_id = 0;
	pos_msg.x = x;
	pos_msg.y = y;
	pos_msg.z = z;
	pub.publish(pos_msg);
}

void do_ultrasonic(ros::Publisher &pub, U8 range, U8 angle){
	std::cout<<"ultrasonic : range="
			 <<range<<",angle="
			 <<angle<<std::endl;
}

void do_gyroscope(ros::Publisher &pub, U8 h_x, U8 l_x, U8 h_y,
                  U8 l_y, U8 h_z, U8 l_z, U8 h_av, U8 l_av)
{
    

}

void do_geomagnetic(ros::Publisher &pub, const SensorData &data){
	// Build message
	//
	// Publish message
	// pub.pulish();
	printf("recv> %2x %2x\n", data.geomag.roll_l, data.geomag.roll_h);
	
}


int main(int argc, char **argv)
{
    /* Init Node */
    ros::init(argc, argv, "robot_sensor_node");
    ros::NodeHandle n;

    /* deal with Institution message. */
    Communication com("192.168.1.1",2001);

    /* deal with Sensors message. */
    ros::Publisher laser_pub = n.advertise<sensor_msgs::LaserScan>("scan", 50);
    ros::Publisher geomag_pub = n.advertise<robot_msgs::RobotGeomagnetic>("robot/sensor/geomagnetic", 50);

    while(ros::ok()){
        /* Receive data from robot */
        U8 type;
        SensorData sdata;
        if(!com.ReceiveData(type, sdata)){
			std::cerr<<"Error:Receive sensor data failed"<<std::endl;
			continue;
		}
		
		/* Valid data */
        if(false){
            continue;
        }
        
        /* Foward to the topic */
        if(type==kTypeUltr){  // Ultrasonic data
            // ...
        }else if(type==kTypeGyro){ //
            // ...
        }else if(type==kTypeGeomag){ // Geomagnetic Sensor Data
			do_geomagnetic( geomag_pub, sdata);
		}else if(type==kTypePos){ // Position data
            // ...
   	    }
    }
 
   return 0;
}
